/**
 * @file         gait.c/h
 * @brief        步态生成器
 * @details      步态规划生成以及执行函数
 * @author       ylt
 * @date         2020-6-03
 * @version      V1.0
 * @copyright    Copyright HCRT (c) 2018-2020
 **********************************************************************************
 * @attention
 * 硬件平台:stm32f4
 * SDK版本：halv1.7
 * @par 修改日志:
 * <table>
 * <tr><th>Date        <th>Version  <th>Author    <th>Description
 * <tr><td>2018/08/17  <td>1.0      <td>wanghuan  <td>创建初始版本
 * </table>
 *
 **********************************************************************************
 */
#include "gait.h"
#include "jump.h"

#include <math.h>
#include <stdio.h>
#include <webots/motor.h>
#include <webots/robot.h>

float output_pos[8];
float kp = 100, ki = 0, kd = 0;

LegGain_t gait_gains[] = {
    // kp_pos, kd_pos, kp_spd, kd_spd
    {22.0, 0.00, 8.0, 0.00}, // TROT
    {22.0, 0.00, 8.0, 0.00}, // WALK
    {22.0, 0.00, 8.0, 0.00}, // GALLOP
};

GaitParams_t gait_params[] = {
    // stance_height, step_length, up_amp, down_amp, flight_percent, freq
    //步高, 步长, 抬腿高, 压腿高, 飞行占比, 频率 [cm]
    {{20.0, 10.0, 8.00, 0.00, 0.12, 1.5}, // TROT 双排步态-对角小跑 左后+右前-右后+左前
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5},
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5},
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5}},
    {{20.0, 10.0, 8.00, 0.00, 0.12, 1.5}, // WALK 单拍步态-慢步 左后-左前-右后-右前
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5},
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5},
     {20.0, 10.0, 8.00, 0.00, 0.12, 1.5}},  
    {{20.0, 12.0, 8.00, 2.00, 0.10, 2.0}, // GALLOP 三拍步态-袭步 右后-左后+右前-左前
     {20.0, 12.0, 8.00, 2.00, 0.10, 2.0},
     {20.0, 12.0, 8.00, 2.00, 0.10, 2.0},
     {20.0, 12.0, 8.00, 2.00, 0.10, 2.0}},
};

/*
                                0----3
                                -    -
                                1----2
*/
RobotBase_t robot_base[] = {
    //底盘状态数组，前四个为相位 后四个为正反
    {0.0, 0.5, 0.0, 0.5, 1.0, 1.0, 1.0, 1.0},         // TROT
    {0.0, 0.25, 0.75, 0.5, 1.0, 1.0, 1.0, 1.0},       // WALK
    {0.000, 0.333, 0.666, 0.333, 1.0, 1.0, 1.0, 1.0}, // GALLOP
    //{0.000, 0.25, 0.5, 0.25, 1.0, 1.0, 1.0, 1.0}, // GALLOP
    // {0.333, 0.666, 0.333, 0.000, 1.0, 1.0, 1.0, 1.0}, // GALLOP
                                                      // {0.666, 0.333, 0.000, 0.333, 1.0, 1.0, 1.0, 1.0}, // GALLOP
};

GaitState_e GAIT_STATE = JUMP; //状态机索引

extern float temp_time0;
float temp_time;

void GaitTask(void)
{
  switch (GAIT_STATE)
  {
  case REALSE: // 释放 什么都不做
    break;
  case STOP: //停止
    // temp_time = wb_robot_get_time() - temp_time0;
    // if (temp_time >= 0.8) //等待一算时间再开始下一次跳跃
    // {
      // start_time_ = wb_robot_get_time();
      // GAIT_STATE = JUMP;
      // prep_angle = 34;
      // jump_angle = 45;
      // fall_angle = -15;
      // kp = 80;
    // }
    break;
  case JUMP: //跳跃
      ExecuteJump();
   // BackSomersault();
    break;
  case TROT: //对角小跑
    kp = 30;
    gait(gait_params[GAIT_STATE], gait_gains[GAIT_STATE], robot_base[GAIT_STATE]);
    break;
  case WALK: //跳跃
    kp = 30;
    gait(gait_params[GAIT_STATE], gait_gains[GAIT_STATE], robot_base[GAIT_STATE]);
    break;
  case GALLOP: //跳跃
    kp = 30;
    gait(gait_params[GAIT_STATE], gait_gains[GAIT_STATE], robot_base[GAIT_STATE]);
    break;
  }
}

/**
 * NAME: void gait(	GaitParams_t params,float leg0_offset, float
 * leg1_offset,float leg2_offset, float leg3_offset) FUNCTION : 产生时间脉冲
 * 设定每个腿的参数 调整腿的运行方向 进行补偿
 */
void gait(GaitParams_t params, LegGain_t gains, RobotBase_t robot_base)
{
  // if (!IsValidGaitParams(params) || !IsValidLegGain_t(gains)) {
  //   return;
  // }
  float t = wb_robot_get_time();
  CoupledMoveLeg(t, params.leg0, robot_base.leg0_offset, robot_base.leg0_direction, 0);
  CoupledMoveLeg(t, params.leg1, robot_base.leg1_offset, robot_base.leg1_direction, 1);
  CoupledMoveLeg(t, params.leg2, robot_base.leg2_offset, robot_base.leg2_direction, 2);
  CoupledMoveLeg(t, params.leg3, robot_base.leg3_offset, robot_base.leg3_direction, 3);

  // ChangeTheGainsOfPD(gains);
}

/**
 * NAME: void CoupledMoveLeg(float t, GaitParams params,float gait_offset, float
 * leg_direction, int LegId) FUNCTION : 驱动并联腿 传递参数
 */
void CoupledMoveLeg(float t, GaitParam_t param, float gait_offset, float leg_direction, int leg_id)
{
  float x, y, theta, gamma;
  SinTrajectory(t, param, gait_offset, &x, &y);               //足端摆线轨迹生成器
  CartesianToThetaGamma(x, y, leg_direction, &theta, &gamma); //笛卡尔坐标转换到加马坐标
  SetCoupledPosition(leg_id, theta, gamma);                   //发送数据给电机驱动函数
  // printf("\r\nt=%f x=%f  y=%f  theta1=%f  theta2=%f  legid=%d he%f", t, x, y, theta1, theta2, LegId, theta1 + theta2);
}

/**
 * NAME: SinTrajectory (float t,GaitParams params, float gaitOffset)
 * FUNCTION : 正弦轨迹生成器
 */
void SinTrajectory(float t, GaitParam_t param, float gait_offset, float *x, float *y)
{
  static float p = 0;
  static float prev_t = 0;

  float stanceHeight = param.stance_height;
  float downAMP = param.down_amp;
  float upAMP = param.up_amp;
  float flightPercent = param.flight_percent;
  float stepLength = param.step_length;
  float FREQ = param.freq;

  p += FREQ * (t - prev_t);
  prev_t = t;

  float gp = fmod((p + gait_offset), 1.0);
  if (gp <= flightPercent)
  {
    *x = (gp / flightPercent) * stepLength - stepLength / 2.0;
    *y = -upAMP * sin(PI * gp / flightPercent) + stanceHeight;
  }
  else
  {
    float percentBack = (gp - flightPercent) / (1.0 - flightPercent);
    *x = -percentBack * stepLength + stepLength / 2.0;
    *y = downAMP * sin(PI * percentBack) + stanceHeight;
  }
}

/**
 * NAME: void CartesianToThetaGamma(float leg_direction)
 * FUNCTION : 笛卡尔坐标转换到角度坐标 也就是将xy转换成theta
 */
void CartesianToThetaGamma(float x, float y, float leg_direction, float *theta, float *gamma)
{
  float L = 0, N = 0;
  float A1 = 0, A2 = 0;
  double M = 0;
  L = sqrt(pow(x, 2) + pow(y, 2));
  constrain(L, 10, 30);
  N = asin(x / L) * 180.0 / PI;
  M = acos((pow(L, 2) + pow(L1, 2) - pow(L2, 2)) / (2 * L1 * L)) * 180.0 / PI;
  A1 = M - N;
  A2 = M + N;
  if (leg_direction == 1.0)
  {
    *gamma = (A1 - 90.0);
    *theta = (A2 - 90.0);
  }
  else if (leg_direction == -1.0)
  {
    *theta = (A1 - 90.0);
    *gamma = (A2 - 90.0);
  }
  // printf(
  //     "\r\n x=%f  y=%f  theta1=%f  theta2=%f   he=%f   L=%f  M=%lf N=%f "
  //     "SIXI=%f",
  //     x, y, theta1, theta2, theta1 + theta2, L, M, N,
  //     ((pow(L, 2) + pow(L1, 2) - pow(L2, 2)) / (2 * L1 * L)));
}

/**
 * NAME: void SetCoupledPosition( int LegId)
 * FUNCTION : 发送电机控制角度
 */
void SetCoupledPosition(int leg_id, float theta, float gamma)
{
  if (leg_id == 0)
  {
    output_pos[0] = ANGLE2RAD(theta);
    output_pos[1] = ANGLE2RAD(gamma);
    // printf("!! theta %f gamma %f !!\r\n", theta, gamma);
  }
  else if (leg_id == 1)
  {
    output_pos[2] = ANGLE2RAD(theta);
    output_pos[3] = ANGLE2RAD(gamma);
  }
  else if (leg_id == 2)
  {
    output_pos[4] = ANGLE2RAD(theta);
    output_pos[5] = ANGLE2RAD(gamma);
  }
  else if (leg_id == 3)
  {
    output_pos[6] = ANGLE2RAD(theta);
    output_pos[7] = ANGLE2RAD(gamma);
  }

  WbDeviceTag motor1_1 = wb_robot_get_device("motor1_1");
  WbDeviceTag motor1_2 = wb_robot_get_device("motor1_2");
  WbDeviceTag motor2_1 = wb_robot_get_device("motor2_1");
  WbDeviceTag motor2_2 = wb_robot_get_device("motor2_2");
  // WbDeviceTag motor3_1 = wb_robot_get_device("motor3_1");
  // WbDeviceTag motor3_2 = wb_robot_get_device("motor3_2");
  // WbDeviceTag motor4_1 = wb_robot_get_device("motor4_1");
  // WbDeviceTag motor4_2 = wb_robot_get_device("motor4_2");

  wb_motor_set_position(motor1_1, output_pos[0]);
  wb_motor_set_position(motor1_2, output_pos[1]);
  wb_motor_set_position(motor2_1, output_pos[2]);
  wb_motor_set_position(motor2_2, output_pos[3]);
  // wb_motor_set_position(motor3_1, output_pos[4]);
  // wb_motor_set_position(motor3_2, output_pos[5]);
  // wb_motor_set_position(motor4_1, output_pos[6]);
  // wb_motor_set_position(motor4_2, output_pos[7]);

  wb_motor_set_control_pid(motor1_1, kp, 0, 0);
  wb_motor_set_control_pid(motor1_2, kp, 0, 0);
  wb_motor_set_control_pid(motor2_1, kp, 0, 0);
  wb_motor_set_control_pid(motor2_2, kp, 0, 0);
  // wb_motor_set_control_pid(motor3_1, kp, 0, 0);
  // wb_motor_set_control_pid(motor3_2, kp, 0, 0);
  // wb_motor_set_control_pid(motor4_1, kp, 0, 0);
  // wb_motor_set_control_pid(motor4_2, kp, 0, 0);
}

/**
 * NAME: void CommandAllLegs(void)
 * FUNCTION : 控制所有电机
 */
void CommandAllLegs(float theta, float gamma, LegGain_t gains)
{
  //    if (!IsValidLegGain_t_t(gains)) {
  //        return;
  //    }
  //    //限位保护
  //

  SetCoupledPosition(0, theta, gamma);
  SetCoupledPosition(1, theta, gamma);
  // SetCoupledPosition(2, theta, gamma);
  // SetCoupledPosition(3, theta, gamma);
}

/**
 *
 * 检测步态增益是否正确
 * @param  gains LegGain_t_t to check
 * @return       True if valid gains, false if invalid
 */
// bool IsValidLegGain(LegGain_t gains) {
//   // check for unstable gains
//   bool bad = gains.kp_spd < 0 || gains.kd_spd < 0 || gains.kp_pos < 0 ||
//              gains.kd_pos < 0;
//   if (bad) {
//     printf("Invalid gains: <0");
//     vTaskDelay(500);
//     return false;
//   }
//   // check for instability / sensor noise amplification
//   bad = bad || gains.kp_spd > 50 || gains.kd_spd > 50 || gains.kp_pos > 50 ||
//         gains.kd_pos > 50;
//   if (bad) {
//     printf("Invalid gains: too high.");
//     vTaskDelay(500);
//     return false;
//   }
//   // check for underdamping -> instability
//   bad = bad || (gains.kp_spd > 50 && gains.kd_spd < 0.1);
//   bad = bad || (gains.kp_pos > 50 && gains.kd_pos < 0.1);
//   if (bad) {
//     printf("Invalid gains: underdamped");
//     vTaskDelay(500);
//     return false;
//   }
//   return true;
// }
/**
 *
 * 检测步态参数是否正确
 * @param  gains LegGain_t_t to check
 * @return       True if valid gains, false if invalid
 */
// bool IsValidGaitParams_t(GaitParam_t params) {
//   const float maxL = 30.1;
//   const float minL = 9.9;

//   float stanceHeight = params.stance_height;
//   float downAMP = params.down_amp;
//   float upAMP = params.up_amp;
//   float flightPercent = params.flight_percent;
//   float stepLength = params.step_length;
//   float FREQ = params.freq;

//   if (stanceHeight + downAMP > maxL ||
//       sqrt(pow(stanceHeight, 2) + pow(stepLength / 2.0, 2)) > maxL) {
//     printf("Gait overextends leg");
//     vTaskDelay(500);
//     return false;
//   }
//   if (stanceHeight - upAMP < minL) {
//     printf("Gait underextends leg");
//     vTaskDelay(500);
//     return false;
//   }

//   if (flightPercent <= 0 || flightPercent > 1.0) {
//     printf("Flight percent is invalid");
//     vTaskDelay(500);
//     return false;
//   }

//   if (FREQ < 0) {
//     printf("Frequency cannot be negative");
//     vTaskDelay(500);
//     return false;
//   }

//   if (FREQ > 10.0) {
//     printf("Frequency is too high (>10)");
//     vTaskDelay(500);
//     return false;
//   }

//   return true;
// }

/**
 *改变腿部增益
 *调用了PID_reset_kpKd 函数
 */
// void ChangeTheGainsOfPD(LegGain_t gains) {
//   for (int i = 0; i < 8; i++) {
//     pid_reset_kpkd(&pid_pos[i], gains.kp_pos, gains.kd_pos);
//     pid_reset_kpkd(&pid_spd[i], gains.kp_spd, gains.kd_spd);
//   }
// }